#include <Arduino.h>
#include "SF_Serial.h"


SF_Serial_DATA receive_Data;
SF_Serial_DATA send_data;


uint8_t Serial_Contact(void){
  if (Serial1.available()) {
    char header = Serial1.read();  // Read the header: 'T', 'M', or 'P'
    if (header == 'T') {
      receive_Target_Data();  // Handle target data
    } else if (header == 'M') {
      receive_Mode_Data();  // Handle mode data
    } else if (header == 'P') {
      receive_PID_Data();  // Handle PID data
    }
    return 1;
  }
  return 0;
}

// Function to receive and process target data
void receive_Target_Data() {
  char buffer[50];  // Adjust size as needed
  if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {

    sscanf(buffer, "%f %f", &receive_Data.M0_Target, &receive_Data.M1_Target);
  }
}

SF_Serial_DATA Serial_Data(void){
  return receive_Data;
}


// Function to receive and process mode data
void receive_Mode_Data() {
  char buffer[50];  // Adjust size as needed
  if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {
    sscanf(buffer, "%f %f", &receive_Data.M0_Mode, &receive_Data.M1_Mode);
  }
}



// Function to receive and process PID data
void receive_PID_Data() {
  char buffer[50];  // Adjust size as needed
  if (Serial1.readBytesUntil('\n', buffer, sizeof(buffer))) {
    sscanf(buffer, "%f %f %f %f %f %f", &receive_Data.M0_P, &receive_Data.M0_I, &receive_Data.M0_D, &receive_Data.M1_P, &receive_Data.M1_I, &receive_Data.M1_D);
  }
}



// Function to send motor data to Python
void send_Motor_Data(float M0_angle,float M0_speed,float M1_angle,float M1_speed) {
  // motorData.M0_angle = 45.0;  // Sample data, replace with actual values
  // motorData.M0_speed = 100.0;
  // motorData.M1_angle = 60.0;
  // motorData.M1_speed = 120.0;

  send_data.M0_angle = M0_angle;  // Sample data, replace with actual values
  send_data.M0_speed = M0_speed;
  send_data.M1_angle = M1_angle;
  send_data.M1_speed = M1_speed;
  
  // Sending formatted data to Python
  Serial1.printf("D%f %f %f %f\n", send_data.M0_angle, send_data.M0_speed, send_data.M1_angle, send_data.M1_speed);
}

